//LCD4096
//a little hack to use full 4096 palette in 256color mode

#include "lcd.h"

//routines to communicate with LCD
void sendData(byte data);
void sendCMD(byte data);

byte PAL[20]={0,2,4,6,8,10,12,15,0,2,4,6,8,10,12,15,0,4,9,15};
byte LRU[20]={0,2,3,4,5,6,7,1,0,2,3,4,5,6,7,1,0,2,3,1};

#define NOT_FOUND 0xff

void LCD_cpal(byte *pal){
	int i;
	sendCMD(RGBSET8);   // setup color lookup table
	for (i=0;i<20;i++) sendData(pal[i]); //send whole palette table
}

void LCD_cinit(void){
  LCD_cpal(PAL);
} 

void LRU8(byte *lru, byte num){
 int i;
 for(i=0;i<8;i++){lru[i]++;}
 lru[num]=0;
}
void LRU4(byte *lru, byte num){
 int i;
 for(i=0;i<4;i++){lru[i]++;}
 lru[num]=0;
}

byte LRUget(byte *lru,byte num){
 int i,max=0,pos=0;
 for(i=0;i<num;i++){
  if (max<lru[i]){max=lru[i];pos=i;}
 }
 return pos;
}

byte LRUlook(byte *pal,byte num, byte value){
 int i;
 for(i=0;i<num;i++){
  if (pal[i]==value) return i;
 }
 return NOT_FOUND;
}

byte LCD_color(byte r,byte g, byte b){
 byte rr,gg,bb,dirty=0;

 rr=LRUlook(PAL,8,r);
 if (rr==NOT_FOUND)	{
  rr=LRUget(LRU,8);
  PAL[rr]=r;
  dirty=1;
 }
 LRU8(LRU,rr);

 gg=LRUlook(PAL+8,8,g);
 if (gg==NOT_FOUND)	{
  gg=LRUget(LRU+8,8);
  PAL[gg+8]=g;
  dirty=1;
 }
 LRU8(LRU+8,gg);

 bb=LRUlook(PAL+16,4,b);
 if (bb==NOT_FOUND)	{
  bb=LRUget(LRU+16,4);
  PAL[bb+16]=b;
  dirty=1;
 }
 LRU4(LRU+16,bb);

 if (dirty) LCD_cpal(PAL);

 return (bb&0x03) | ((gg&0x07)<<2) | ((rr&0x07)<<5);

}
